//------------------------------------------------------------------------------
// File:          flash.c
// Function:      Flash functions
// Supported chip(s):
//    - SAM3X8
//    - SAM3X4
//    - SAM3X2
// Supported toolchain(s):
//    - IAR Embedded Workbench
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

#include "flash.h"
#include "flashd.h"
//#include "uart_api.h"

#ifdef USE_FLASH

//------------------------------------------------------------------------------
// Functions
//------------------------------------------------------------------------------

/**
 * Name:      flash_init
 * Purpose:   Initializes the flash (nothing to do)
 */
void flash_init() {

//  UART_send_string("Flash: Initializing ...\n\r");
  FLASHD_Initialize(CHIP_FREQ_CPU_MAX, 1);
//  UART_send_string("Flash: Initialization done.\n\r");
}

/**
 * Name:      flash_cleanup
 * Purpose:   Cleans up the flash (nothing to do)
 */
void flash_cleanup() {

//  UART_send_string("Flash: Cleaning up ...\n\r");
//  UART_send_string("Flash: Cleanup done.\n\r");
}

/**
 * Name:      flash_write
 * Purpose:   Writes a page in the flash memory
 * Inputs:
 *  - Destination address in the flash
 *  - Source data buffer
 * Output: OK if write is successful, ERROR otherwise
 */
unsigned int flash_write(void * vAddress, void * vData) {

    uint32_t rc;
    __DSB();
    __ISB();
    __disable_irq();
    __DMB();
    rc = FLASHD_Write((uint32_t)vAddress, vData, IFLASH_PAGE_SIZE);
    __DMB();
    __DSB();
    __ISB();
    __enable_irq();
    if (rc)
        return ERROR;
    return OK;
}

/**
read a page from flash
input:vAddress--flash read start address
      buf--data buf
**/
/*void flash_read(void *vAddress,uint8_t *buf)
{
    uint16_t i;
    uint8_t *p_data = (uint8_t *)vAddress;
    
    for(i=0;i<IFLASH_PAGE_SIZE;i++)
    {
        buf[i] = p_data[i];
    }
}*/

/**
compare a page data with the writen buf
input:vAddress--flash read start address
      buf--the data buf to be compared
output:OK--if the data in buf is the same as read from flash
       ERROR--if different
**/
/*uint32_t flash_compare(void *vAddress,uint8_t *buf)
{
    uint8_t *p_data = (uint8_t *)vAddress;
    uint16_t i;
    
    for(i=0;i<IFLASH_PAGE_SIZE;i++)
    {
        if(p_data[i] != buf[i])
        {
            return(ERROR);
        }
    }
    return(OK);
}*/

/**
 * Name:      flash_next
 * Purpose:   Computes next page address after (or starting at) specified address
 * Input:
 *  - Address
 * Output:    Next page address
 */
void * flash_next(void * vAddress) {

  // Local variables
  unsigned int address = (unsigned int) vAddress;

  if (address%MEMORY_PAGE_SIZE != 0) {

    address += (MEMORY_PAGE_SIZE - address%MEMORY_PAGE_SIZE);
  }

  // Check if address is remapped
  if (address < (unsigned int) MEMORY_START_ADDRESS) {

    address += (unsigned int) MEMORY_START_ADDRESS;
  }

  return (void *) address;
}

/**
 * Name:      flash_lock
 * Purpose:   Sets the lock bit for the specified memory region
 * Input:
 *  - Start address of region
 *  - End address of region
 * Output:    OK if lock is successful, ERROR otherwise
 */
unsigned int flash_lock(void * vStart, void * vEnd) {

    uint32_t uStart = (uint32_t)vStart, uEnd = (uint32_t)vEnd;
    uint32_t rc;
    if (uStart < IFLASH0_ADDR)
        uStart = IFLASH0_ADDR;
    if (uEnd > IFLASH_END || uEnd == 0)
        uEnd = IFLASH_END;
    if (uStart >= uEnd)
        return ERROR;
    __DSB();
    __ISB();
    __disable_irq();
    __DMB();
    rc = FLASHD_Lock(uStart, uEnd, NULL, NULL);
    __DMB();
    __DSB();
    __ISB();
    __enable_irq();
    if (rc)
        return ERROR;
    return OK;
}

/**
 * Name:      flash_unlock
 * Purpose:   Clears the lock bit for the specified memory region
 * Input:
 *  - Start address of the region to unlock
 *  - End address of the region to lock
 * Output:    OK if lock is successful, ERROR otherwise
 */
unsigned int flash_unlock(void * vStart, void * vEnd) {

    uint32_t uStart = (uint32_t)vStart, uEnd = (uint32_t)vEnd;
    uint32_t rc;
    if (uStart < IFLASH0_ADDR)
        uStart = IFLASH0_ADDR;
    if (uEnd > IFLASH_END || uEnd == 0)
        uEnd = IFLASH_END;
    if (uStart >= uEnd)
        return ERROR;
    __DSB();
    __ISB();
    __disable_irq();
    __DMB();
    rc = FLASHD_Unlock(uStart, uEnd, NULL, NULL);
    __DMB();
    __DSB();
    __ISB();
    __enable_irq();
    if (rc)
        return ERROR;
    return OK;
}

void _binExec (void * l_code_addr)
{
    __asm ("mov     r1, r0        \n"
           "ldr     r0, [r1, #4]  \n"
               "ldr     sp, [r1]      \n"
                   /*"msr     msp, [r1]     \n"*/
                   "blx     r0"
                       );
}

int binary_exec(void * vStart)
{
    int i;
    
    // -- Check parameters
    // Should be at least 32 words aligned
    if ((uint32_t)vStart & 0x7F)
        return 1;
    // Should in code or sram region
    if ((uint32_t)vStart > CM3_SRAM_END)
        return 2;
    
    // -- Disable interrupts
    // Disable IRQ
    __disable_irq();
    // Disable IRQs
    for (i = 0; i < 8; i ++) NVIC->ICER[i] = 0xFFFFFFFF;
    // Clear pending IRQs
    for (i = 0; i < 8; i ++) NVIC->ICPR[i] = 0xFFFFFFFF;
    
    // -- Modify vector table location
    // Barriars
    __DSB();
    __ISB();
    // Change the vector table
    SCB->VTOR = ((uint32_t)vStart & SCB_VTOR_TBLOFF_Msk);
    // Barriars
    __DSB();
    __ISB();
    
    // -- Enable interrupts
    __enable_irq();
    
    // -- Load Stack & PC
    _binExec(vStart);
    
    return 0;
}

#endif // USE_FLASH
